// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/float64.hpp"

#include <std_msgs/msg/int32.hpp>
#include "std_msgs/msg/int32_multi_array.hpp"

int state1 = 0;
// ros2 bag record /joint1_p /joint2_p /joint3_p /joint4_p /y_ros2msg /big_valve_state /end_action

// 3.自定义节点类；
class PocmsgNode : public rclcpp::Node
{
public:
    PocmsgNode() : Node("pocmsgnode_node_cpp")
    {
        // 订阅位置消息
        RCLCPP_INFO(this->get_logger(), "pocmsg node has been started.");
        sub_ = this->create_subscription<std_msgs::msg::Float32>(
            "/y_ros2msg", 10,
            std::bind(&PocmsgNode::topic_callback, this, std::placeholders::_1));
        // 发布位置消息
        joint1_pub_ = this->create_publisher<std_msgs::msg::Float64>("joint1_gp", 10);
        joint2_pub_ = this->create_publisher<std_msgs::msg::Float64>("joint2_gp", 10);
        joint3_pub_ = this->create_publisher<std_msgs::msg::Float64>("joint3_gp", 10);
        joint4_pub = this->create_publisher<std_msgs::msg::Int32>("joint4_gp", 10);
        // 阀门控制
        valve_pub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("big_valve_state", 10);
        // 扫刷控制
        endaction_pub = this->create_publisher<std_msgs::msg::Int32>("end_action", 10);
        run_model_pub_ = this->create_publisher<std_msgs::msg::Int32>("run_model_sub", 10);
    }

private:
    // 订阅回调函数
    void topic_callback(const std_msgs::msg::Float32::SharedPtr msg) const
    {
        float loc_y = msg->data;
        int state = -1;
        // 一号关节位置
        auto joint1_message = std_msgs::msg::Float64();
        // 二号关节位置
        auto joint2_message = std_msgs::msg::Float64();
        // 三号关节位置
        auto joint3_message = std_msgs::msg::Float64();
        // 四号关节位置
        auto joint4_message = std_msgs::msg::Int32();
        // 阀体控制
        auto serial_data = std_msgs::msg::Int32MultiArray();
        serial_data.data.resize(4);
        // 扫刷控制
        auto endact_data = std_msgs::msg::Int32();
        auto run_model = std_msgs::msg::Int32();

        // 消防前
        if (loc_y < 12)
        {
            state = 0;
            // 关节角度
            joint1_message.data = 52;
            // 油缸1长度
            joint2_message.data = 45;
            // 油缸2长度
            joint3_message.data = 30;
            // 关节4角度
            joint4_message.data = 0;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }
        // 躲避消防
        else if (loc_y > 12 && loc_y < 19)
        {
            state = 1;
            state1 = 1;
            // 关节角度
            joint1_message.data = 74;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }
        // 水坑1
        else if (loc_y > 19 && loc_y < 21 && state1 == 0)
        {
            state = 2;
            // 关节角度
            joint1_message.data = 48;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }
        // 水坑2
        else if (loc_y > 21 && loc_y < 23)
        {
            state1 = 1;
            state = 3;
            // 关节角度
            joint1_message.data = 42;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }
        // 水坑3
        else if (loc_y > 20 && loc_y < 24 && state1 == 1)
        {
            state = 3;
            // 关节角度
            joint1_message.data = 37;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }
        // water 4
        else if (loc_y > 25 && loc_y < 32)
        {
            state = 4;
            // 关节角度
            joint1_message.data = 53;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }
        //
        else if (loc_y > 32 && loc_y < 38)
        {
            state = 5;
            // 关节角度
            joint1_message.data = 55;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }
        else if (loc_y > 40 && loc_y < 60)
        {
            state = 5;
            // 关节角度
            joint1_message.data = 42;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }

        else if (loc_y > 60)
        {
            state = 5;
            // 关节角度
            joint1_message.data = 74;
            // 低压
            serial_data.data[0] = 0;
            serial_data.data[1] = 0;
            // 高压
            serial_data.data[2] = 3;
            serial_data.data[3] = 3;
            // 扫刷
            endact_data.data = 1;
            // 高压泵
            run_model.data = 2;
        }

        RCLCPP_INFO(this->get_logger(), "当前 x 轴位置: 'y = %.2f'，状态：%d", loc_y, state);
        // 发布
        joint1_pub_->publish(joint1_message);
        joint2_pub_->publish(joint2_message);
        joint3_pub_->publish(joint3_message);
        joint4_pub->publish(joint4_message);
        // RCLCPP_INFO(this->get_logger(), "Published joint1 position: 'joint1 = %.2f'", joint1_message.data);
        // 阀体控制
        valve_pub_->publish(serial_data);
        // RCLCPP_INFO(this->get_logger(), "Published valve states: [%d, %d, %d, %d]", serial_data.data[0], serial_data.data[1], serial_data.data[2], serial_data.data[3]);
        // 扫刷控制
        endaction_pub->publish(endact_data);
        // RCLCPP_INFO(this->get_logger(), "Published end action state: %d", endact_data.data);
        run_model_pub_->publish(run_model);
    }
    // 订阅话题
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_;
    // 发布话题
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint1_pub_;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint2_pub_;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr joint3_pub_;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr joint4_pub;
    // 阀门控制
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr valve_pub_;
    // 扫刷控制
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr endaction_pub;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr run_model_pub_;
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<PocmsgNode>());
    // 5.资源释放
    rclcpp::shutdown();
    return 0;
}